package Demo;

import java.util.Vector;

import org.jbox2d.collision.CircleDef;
import org.jbox2d.collision.CircleShape;
import org.jbox2d.collision.PolygonDef;
import org.jbox2d.collision.PolygonShape;
import org.jbox2d.collision.ShapeType;
import org.jbox2d.common.Color3f;
import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.BodyDef;
import org.jbox2d.dynamics.DebugDraw;
import org.jbox2d.dynamics.World;
import org.jbox2d.dynamics.joints.Joint;
import org.jbox2d.dynamics.joints.RevoluteJointDef;
import org.jbox2d.p5.Physics;
import org.jbox2d.testbed.ProcessingDebugDraw;

import processing.core.PApplet;


public class GADemo extends PApplet
{
	private static final long serialVersionUID = -6603773936189530403L;

	
	@Override
	public String getName() 
	{
		return "GADemo";
	}

	private World world;
	private Physics physics;
	float x1, y1, x2, y2;
	private boolean testFinished = true;
	private int testCount = 0;
	private int solutionCount = -1;
	private int generationCount = 1;
	private Solution solutions[] = new Solution[8];
	private int selectMethod = 0;
	private int breedMethod = 0;
	private Vector<Float> terrainValues = new Vector<Float>();
	private float terrainHeight = 0.1f;
	private Body testBody;
	private int worldLengthX = 20000;
	private int worldLengthY = 1000;
	private DebugDraw ddraw;
	private Vec2 lastPos = new Vec2(-5,-5);
	public void draw()
	{
		if(!testFinished)
		{
			world.step(1f/30f, 10);
			
			int score = (int) testBody.getWorldPoint(testBody.getLocalCenter()).x;
			if(score > solutions[solutionCount].fitness)
			{
				solutions[solutionCount].fitness = score;
			}
			
			testCount++;
			if((testBody.getLinearVelocity().x < 0.01 && testBody.getLinearVelocity().x > -0.01 && testCount > 150) || (testBody.getWorldPoint(testBody.getLocalCenter()).x == lastPos.x && testBody.getWorldPoint(testBody.getLocalCenter()).y == lastPos.y) || testBody.getWorldPoint(testBody.getLocalCenter()).x >= 1000 || testBody.getWorldPoint(testBody.getLocalCenter()).x <= -5 )
			{
				testFinished = true;
				
			}
			
			lastPos = testBody.getWorldPoint(testBody.getLocalCenter());
		}
		else
		{
			solutionCount++;
			
			for (Body body = physics.getWorld().getBodyList(); body != null; body = body.getNext()) 
			{
				world.destroyBody(body);
			}
			for (Joint body = physics.getWorld().getJointList(); body != null; body = body.getNext()) 
			{
				world.destroyJoint(body);
			}
			
			if(solutionCount>7)
			{
				Selection sel = new Selection(solutions);
				if(selectMethod == 0)
				{
					solutions = sel.rouleteSelection();
				}
				else if(selectMethod == 1)
				{
					solutions = sel.tournamentSelection(2);
				}
				
				for( int j = 0; j < 4 ;j++)
				{
					if(breedMethod==0)
					{
						String[] strings = Crossover.OnePoint(solutions[j].chromosome, solutions[j+1].chromosome, (int) ((Math.random()*solutions[j].chromosome.length()-2)+1));
						strings[0] = Crossover.Mutate(strings[0],.05);
						strings[1] = Crossover.Mutate(strings[1],.05);
						solutions[j] = new Solution(strings[0]);
						solutions[j+1] = new Solution(strings[1]);
					}
					if(breedMethod==1)
					{
						String[] strings = Crossover.TwoPoint(solutions[j].chromosome, solutions[j+1].chromosome, (int) ((Math.random()*solutions[j].chromosome.length()-2)+1), (int) ((Math.random()*solutions[j].chromosome.length()-2)+1));
						strings[0] = Crossover.Mutate(strings[0],.05);
						strings[1] = Crossover.Mutate(strings[1],.05);
						solutions[j] = new Solution(strings[0]);
						solutions[j+1] = new Solution(strings[1]);
					}
					if(breedMethod==2)
					{
						String[] strings = Crossover.Uniform(solutions[j].chromosome, solutions[j+1].chromosome);
						strings[0] = Crossover.Mutate(strings[0],.05);
						strings[1] = Crossover.Mutate(strings[1],.05);
						solutions[j] = new Solution(strings[0]);
						solutions[j+1] = new Solution(strings[1]);
					}
				}
				
				solutionCount = 0;
				generationCount++;
			}
			
			generateTerrain();
			testBody = generateFromSolution(new Solution());
			testBody.allowSleeping(true);
			testCount = 0;
			testFinished = false;
		}
		
		
		
	}
	
	public void generateTerrain()
	{
		terrainHeight = .1f;
		int i;
		for(i = 1; i < terrainValues.size();i++ )
		{
			PolygonDef floorPoly = new PolygonDef();
			floorPoly.density = 1.0f;
			floorPoly.friction=2f;
			floorPoly.addVertex(new Vec2(0,terrainHeight - .7f));
			floorPoly.addVertex(new Vec2(5,terrainHeight + terrainValues.get(i) - .7f));
			floorPoly.addVertex(new Vec2(5, terrainHeight + terrainValues.get(i)));
			floorPoly.addVertex(new Vec2(0, terrainHeight));
			BodyDef floorDef = new BodyDef();
			terrainHeight += terrainValues.get(i);
			floorDef.position = new Vec2((i-2)*5,0);
			Body floor = world.createBody(floorDef); 
			floor.createShape(floorPoly);
		}
		
		PolygonDef floorPoly = new PolygonDef();
		floorPoly.density = 1.0f;
		floorPoly.friction=2f;
		floorPoly.addVertex(new Vec2(0,terrainHeight - .7f));
		floorPoly.addVertex(new Vec2(10,terrainHeight - .7f));
		floorPoly.addVertex(new Vec2(10, terrainHeight));
		floorPoly.addVertex(new Vec2(0, terrainHeight));
		BodyDef floorDef = new BodyDef();
		floorDef.position = new Vec2((i-2)*5,0);
		Body floor = world.createBody(floorDef); 
		floor.createShape(floorPoly);
		
		PolygonDef endPoly = new PolygonDef();
		endPoly.density = 1.0f;
		endPoly.friction=2f;
		endPoly.addVertex(new Vec2(0,- this.worldLengthX));
		endPoly.addVertex(new Vec2(50,- this.worldLengthX));
		endPoly.addVertex(new Vec2(50, this.worldLengthX));
		endPoly.addVertex(new Vec2(0, this.worldLengthX));
		BodyDef endDef = new BodyDef();
		endDef.position = new Vec2((i-2)*5 + 10,0);
		Body end = world.createBody(endDef); 
		end.createShape(endPoly);
	}
	
	public void generateTerrainValues()
	{
		terrainValues.clear();
		terrainValues.add((float) 0);
		terrainValues.add((float) 0);
		terrainValues.add((float) 0);
		for (int i = 0; i < 198 ; i++)
		{
			terrainValues.add((float) (Math.random() * 6 - 3f));
		}
		
	}
	
	public Body generateFromSolution(Solution s)
	{
		CircleDef wheel1Shape = new CircleDef();
		wheel1Shape.friction=5;
		wheel1Shape.density=1;
		wheel1Shape.radius=(float)s.wheel1/10 + 0.1f;
		BodyDef wheel1BodyDef = new BodyDef();
		wheel1BodyDef.position = new Vec2(-s.side1/5,s.height1/3 + 5);
		Body wheel1 = world.createBody(wheel1BodyDef);
		wheel1.createShape(wheel1Shape);
		wheel1.setMassFromShapes();
		

		CircleDef wheel2Shape = new CircleDef();
		wheel2Shape.friction=5;
		wheel2Shape.density=1;
		wheel2Shape.radius=(float)s.wheel2/10 +0.1f;
		BodyDef wheel2BodyDef = new BodyDef();
		wheel2BodyDef.position = new Vec2(s.side2/5,s.height2/3 + 5);
		Body wheel2 = world.createBody(wheel2BodyDef);
		wheel2.createShape(wheel2Shape);
		wheel2.setMassFromShapes();
		
		PolygonDef leftbodyShape = new PolygonDef();
		leftbodyShape.density = 1.0f;
		leftbodyShape.friction=0.5f;
		leftbodyShape.addVertex(new Vec2(0,0));
		leftbodyShape.addVertex(new Vec2(0,(float)s.sharedSide/10));
		leftbodyShape.addVertex(new Vec2(-s.side1/5,s.height1/3));
		
		PolygonDef rightbodyShape = new PolygonDef();
		rightbodyShape.density = 1.0f;
		rightbodyShape.friction=0.5f;
		rightbodyShape.addVertex(new Vec2(0,0));
		rightbodyShape.addVertex(new Vec2(s.side2/5,s.height2/3));
		rightbodyShape.addVertex(new Vec2(0,(float)s.sharedSide/10));
		
		BodyDef bodyDef = new BodyDef();
		bodyDef.position = new Vec2(0,5);
		Body body = world.createBody(bodyDef);
		body.createShape(leftbodyShape);
		body.createShape(rightbodyShape);
		body.setMassFromShapes();


	
		/*PolygonDef floorPoly = new PolygonDef();
		floorPoly.density = 1.0f;
		floorPoly.friction=0.5f;
		floorPoly.setAsBox(10, 1);
		BodyDef floorDef = new BodyDef();
		floorDef.position = new Vec2(0,0);
		Body floor = world.createBody(floorDef);
		floor.createShape(floorPoly);*/
		
		RevoluteJointDef rjl = new RevoluteJointDef();
		rjl.initialize(wheel1, body, new Vec2(-s.side1/5,s.height1/3 + 5));
		rjl.motorSpeed = 50;
		rjl.maxMotorTorque = 5;
		rjl.enableMotor = true;
		rjl.enableLimit = false;
		world.createJoint(rjl);
		
		RevoluteJointDef rjr = new RevoluteJointDef();
		rjr.initialize(wheel2, body, new Vec2(s.side2/5,s.height2/3  + 5));
		rjr.motorSpeed = 50;
		rjr.maxMotorTorque = 5;
		rjr.enableMotor = true;
		rjr.enableLimit = false;
		world.createJoint(rjr);
		
		return body;
	}
	
	@Override
	public void setup() 
	{
		 size(600, 300, JAVA2D);
		 frameRate(30);
		 physics = new Physics(this, worldLengthX, worldLengthY);
		 physics.setDensity(0.1f);
		 physics.setCustomRenderingMethod(this, "sketchBlocks");
		 physics.removeBorder();
		 world = physics.getWorld();
		 world.setGravity(new Vec2(0,-10));
		 
		for(int i = 0; i < 8 ; i++)
		{
			solutions[i] = new Solution();
		}
		generateTerrainValues();
		
		ddraw = new ProcessingDebugDraw(this);
	}
	
	public void resetSim()
	{

		for(int i = 0; i < 8 ; i++)
		{
			solutions[i] = new Solution();
		}
		generateTerrainValues();
		testFinished = true;
		testCount = 0;
		solutionCount = -1;
		generationCount = 0;
	}
	
	public void keyPressed()
	{
		if(key == 'k')
		{
			testFinished = true;
		}
		else if(key == 'r')
		{
			resetSim();
		}
		else if(key == 's')
		{
			this.selectMethod++;
			this.selectMethod = this.selectMethod%2;
		}
		else if(key == 'c')
		{
			this.breedMethod++;
			this.breedMethod = this.breedMethod%3;
		}
	}
	
	public void sketchBlocks(World w)
	{
		float centerPosX = physics.worldToScreen(testBody.getWorldPoint(testBody.getLocalCenter())).x - (this.getSize().width / 2);
		float centerPosY = physics.worldToScreen(testBody.getWorldPoint(testBody.getLocalCenter())).y - (this.getSize().height / 2);
		
		 background(255);
		 
		 for (Body body = physics.getWorld().getBodyList(); body != null; body = body.getNext()) {
		   org.jbox2d.collision.Shape shape;
		   for (shape = body.getShapeList(); shape != null; shape = shape.getNext()) {
		     beginShape();
		     if (shape.getType() == ShapeType.POLYGON_SHAPE) {
		    	fill(150);
		    	if(body.equals(testBody))
		    	{
		    		fill(solutions[solutionCount].wheelColor.getRed(),solutions[solutionCount].wheelColor.getGreen(),solutions[solutionCount].wheelColor.getBlue()); stroke(0);
		    	}
		    	PolygonShape poly = (PolygonShape)shape;
		       int count = poly.getVertexCount();
		       Vec2[] verts = poly.getVertices();
		       for(int i = 0; i < count; i++) {
		         Vec2 vert = physics.worldToScreen(body.getWorldPoint(verts[i]));
		         vertex(vert.x - centerPosX, vert.y - centerPosY);
		       }
		       Vec2 firstVert = physics.worldToScreen(body.getWorldPoint(verts[0]));
		       vertex(firstVert.x - centerPosX, firstVert.y - centerPosY);
		     }
		     if(shape.getType() == ShapeType.CIRCLE_SHAPE){
		    	 fill(solutions[solutionCount].color.getRed(),solutions[solutionCount].color.getGreen(),solutions[solutionCount].color.getBlue()); stroke(0);
		    	CircleShape circle = (CircleShape) shape;
				Vec2 center = circle.getLocalPosition();
				Vec2 wpoint = physics.worldToScreen(body.getWorldPoint(center));
				float radius = physics.worldToScreen(circle.getRadius());
				ellipse(wpoint.x - centerPosX ,wpoint.y - centerPosY,radius*2,radius*2);
		     }
		     endShape();
		   }
		 }
		 
		 ddraw.drawString(10, 20, "Distance : ", new Color3f(100.0f, 100.0f, 100.0f));
		 ddraw.drawString(10, 40, "Generation : ", new Color3f(100.0f, 100.0f, 100.0f));
		 ddraw.drawString(10, 60, "Solution : ", new Color3f(100.0f, 100.0f, 100.0f));
		 
		 ddraw.drawString(80, 20, "" + testBody.getWorldPoint(testBody.getLocalCenter()).x, new Color3f(100.0f, 100.0f, 100.0f));
		 ddraw.drawString(80, 40, "" + generationCount, new Color3f(100.0f, 100.0f, 100.0f));
		 ddraw.drawString(80, 60, "" + (solutionCount+1), new Color3f(100.0f, 100.0f, 100.0f));
		 	
		 ddraw.drawString(160, 20, "Selection Method :", new Color3f(100.0f, 100.0f, 100.0f));
		 ddraw.drawString(160, 40, "Crossover Method :", new Color3f(100.0f, 100.0f, 100.0f));
		 
		 if(selectMethod==0)
		 {
			 ddraw.drawString(270, 20, "Roulette ", new Color3f(100.0f, 100.0f, 100.0f)); 
		 }
		 else if(selectMethod == 1)
		 {
			 ddraw.drawString(270, 20, "Tournament", new Color3f(100.0f, 100.0f, 100.0f));  
		 }
		 
		 if(breedMethod==0)
		 {
			 ddraw.drawString(270, 40, "One Point ", new Color3f(100.0f, 100.0f, 100.0f)); 
		 }
		 else if(breedMethod == 1)
		 {
			 ddraw.drawString(270, 40, "Two Point", new Color3f(100.0f, 100.0f, 100.0f));  
		 }
		 else if(breedMethod == 2)
		 {
			 ddraw.drawString(270, 40, "Uniform", new Color3f(100.0f, 100.0f, 100.0f));  
		 }
		 
	}
	
}
